﻿#include "AxRobotOdometry.h"
#include "math\euler.h"
#include "utils.h"

AxRobotOdometry::AxRobotOdometry(void) 
	:flag(-1), linkFlag(0)
	, safedrive(true), grid(false)
	, mobileSim(true), m_Laser(0)
	, m_threadRunning(false), stopped(true)
	, laserReadingNo(0)
	, firstLaser(0), firstMoveCtrl(0), m_isGoalSet(false)
	, oldDistance(0)
	, m_sensorMap(NULL)
{
}

AxRobotOdometry::~AxRobotOdometry(void)
{
}

void AxRobotOdometry::setExtrapolator(AxExtrapolator* extrapolator_)
{
	extrapolator = extrapolator_;
}

void AxRobotOdometry::startRobot()
{
	global_Mutex.lock();
	is_pause=false;
	global_Mutex.unlock();
	start();
}

void AxRobotOdometry::run(void)
{
	Aria::init();
	int argc=5; 
	char *argv[6]; //argc是字符串个数，argv是字符串内容
	argv[0]="./ArSLAMPCC.exe";
	argv[1]="-host";                                                           
	argv[2] = "localhost";// (LPSTR)(LPCTSTR)Global_IP; //Sim-192.168.244.128/ LX-213.111     
	argv[3]="-port";                                                          
	argv[4]="7272";
	argv[5]=0;

	/*ArClientBase client;*/

	/* Aria components use this to get options off the command line: */
	ArArgumentParser parser(&argc, argv);
	/*ArArgumentBuilder  *arg1=new ArArgumentBuilder();
	ArArgumentParser parser(arg1);*/

	/* This will be used to connect our client to the server, including
	* various bits of handshaking (e.g. sending a password, retrieving a list
	* of data requests and commands...)
	* It will get the hostname from the -host command line argument: */
	ArClientSimpleConnector clientConnector(&parser);
	
	parser.loadDefaultArguments();

	/* Check for -help, and unhandled arguments: */
	if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
	{
		Aria::logOptions();
		exit(0);
	}
  
	/* Connect our client object to the remote server: */	
	if (clientConnector.connectClient(&client)) 
		linkFlag=1;
	QString G_Warning;
	if (linkFlag!=1)
	{
		linkFlag=99;

		if (client.wasRejected())
			G_Warning = "Server rejected connection";
		else
			G_Warning = "Could not connect to server";

		ArUtil::sleep(3000);
		exit(1);
	}

	client.runAsync();

	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	/*ArGlobalFunctor escapeCB(&escape);
	keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);*/

	InputHandler inputHandler(&client);

	inputHandler.safeDrive();
	

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> robotInfoCB(this,&AxRobotOdometry::robotInfo);
	client.addHandler("update", &robotInfoCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> laserCurrentCB(this,&AxRobotOdometry::laserCurrent);
	client.addHandler("getSensorCurrent",&laserCurrentCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> getLocStateCB(this,&AxRobotOdometry::getLocState);
	client.addHandler("getLocState", &getLocStateCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> getMapCB(this,&AxRobotOdometry::getMap);
	client.addHandler("getMap", &getMapCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> getLocPointsCB(this,&AxRobotOdometry::getLocPoints);
	client.addHandler("getLocPoints",&getLocPointsCB);


	/* sending requests to server */
	//client.requestOnce("getMapName");
	//client.requestOnce("getGoals");
	//client.request("update",TimeInterval);
	//client.request("getLocState",TM-2);
	//client.request("pathPlannerStatus",TM-3);
	
	//-------- sim_S3Series_1 for mobileSim use only --------//
	ArNetPacket requestPacket;
	if(mobileSim==true) requestPacket.strToBuf("sim_lms1xx_1"); 
	//if(mobileSim==false) requestPacket.strToBuf("S3Series_1"); 
	//client.request("getSensorCurrent",TM-4,&requestPacket); 
	//client.request("getPath",TM*2-5);

	
	if(client.dataExists("popupClicked"))
	{
		ArNetPacket popupClick;
		popupClick.byte4ToBuf(0);
		popupClick.byteToBuf(1);
		client.requestOnce("popupClicked",&popupClick);
	}

	if(client.dataExists("autoDockDisable"))
	{
		client.requestOnce("autoDockDisable");
	}

	while(client.getRunningWithLock())
	{
		ArNetPacket pkt, pkt_goal, pkt_map;
		global_Mutex.lock();
		if (flag!=ROBOT_STOP)
		{
			//请求机器人状态信息
			client.requestOnce("update");
			//请求机器人激光传感器信息
			if(mobileSim==true)
			{
				client.requestOnce("getSensorCurrent",&requestPacket);	
			}
		}
		switch(flag)
		{
			case ROBOT_UP:inputHandler.up();break;  //if(GetAsyncKeyState(VK_LBUTTON)&0x8000)
			case ROBOT_DOWN:inputHandler.down();break;
			case ROBOT_LEFT:inputHandler.left();break;
			case ROBOT_RIGHT:inputHandler.right();break;
			case ROBOT_STOP:inputHandler.space();break;
			//case 5:client.requestOnce("stop");ArUtil::sleep(25);flag=0;break;  //inputHandler.doStop();
			case ROBOT_LOCALIZETOPOSE:
				{
					pkt.byte4ToBuf(m_Goal.getX());
					pkt.byte4ToBuf(m_Goal.getY());
					pkt.byte4ToBuf(m_Goal.getTh());
					client.requestOnce("axGotoPose",&pkt);
					flag=ROBOT_UNKNOW;
					break; // for mobileSim use ONLY
				}
			case ROBOT_GOTOHEADING:
				{
					//if (m_AjustTruePose)
					{
						pkt.byte4ToBuf(m_Extrapolator_Pose.getX());//m_Extrapolator_Pose在每次读取激光数据的时候更新
						pkt.byte4ToBuf(m_Extrapolator_Pose.getY());
						pkt.byte4ToBuf(m_Extrapolator_Pose.getTh());
					}
					pkt.byte4ToBuf(m_Goal.getX());
					pkt.byte4ToBuf(m_Goal.getY());
					pkt.byte4ToBuf(m_Goal.getTh());
					pkt.byte4ToBuf(control_signal.direction);
					client.requestOnce("axGotoDeltaHeading",&pkt);
					flag=ROBOT_UNKNOW;
					break; // for mobileSim use ONLY
				}
			case ROBOT_AUTOEXPLORER:
				{
					pkt.byte4ToBuf(m_Extrapolator_Pose.getX());//m_TruePose在每次读取激光数据的时候更新
					pkt.byte4ToBuf(m_Extrapolator_Pose.getY());
					pkt.byte4ToBuf(m_Extrapolator_Pose.getTh());
					pkt.byte4ToBuf(m_Goal.getX());
					pkt.byte4ToBuf(m_Goal.getY());
					pkt.byte4ToBuf(m_Goal.getTh());
					pkt.byte4ToBuf(control_signal.direction);
					client.requestOnce("axGotoDeltaHeading",&pkt);
					flag=ROBOT_UNKNOW;
					break; // for mobileSim use ONLY
				}
			case ROBOT_DISCONNECT:client.disconnect();flag=0;break; // disconnect from server
		}
		msleep(200);
		if(safedrive)//安全模式
		{
			inputHandler.safeDrive();
		} 
		else
		{
			inputHandler.unsafeDrive();
		}
		//inputHandler.sendInput(); // don't change this code sequence here!		
		global_Mutex.unlock();
		ArUtil::sleep(100); // set sleep time here to avoid use of too much CPU resource
	}
}


void AxRobotOdometry::setSpeed(double flag_)
{
	global_Mutex.lock();
	deltaSpeed=5;
	global_Mutex.unlock();
}
void AxRobotOdometry::setFlag(int flag_)
{
	global_Mutex.lock();
	flag=flag_;
	global_Mutex.unlock();
}


void AxRobotOdometry::setSafeDrive(bool safe_)
{
	global_Mutex.lock();
	safedrive=safe_;
	global_Mutex.unlock();
}


//运动到鼠标点击的位置
void AxRobotOdometry::setPoseToGoto()
{
	global_Mutex.lock();
	m_GoalSetType=GOAL_SET_TYPE_MOUSE;
	m_isGoalSet=true;
	global_Mutex.unlock();
}

//运动到自主探索的目标点
void AxRobotOdometry::setPoseToExplorer()
{
	global_Mutex.lock();
	m_GoalSetType=GOAL_SET_TYPE_AUTOEXPLORER;
	m_isGoalSet=true;
	global_Mutex.unlock();
}
ArPose AxRobotOdometry::getPoseToExplorer()
{
	m_Goal = extrapolator->getGoal();
	return m_Goal;
}
//计算象限角
static double GetQuadrantAngle(double x, double y)
{
	double theta =atan(y / x);
	if (x == 0 && y == 0) return 0;
	if (x > 0 && y == 0) return 0;
	if (x == 0 && y > 0) return M_PI / 2;
	if (x < 0 && y == 0) return M_PI;
	if (x == 0 && y < 0) return 3 *M_PI / 2;

	if (x > 0 && y > 0) return theta;
	if (x > 0 && y < 0) return M_PI * 2+ theta;
	if (x < 0 && y > 0) return theta + M_PI;
	if (x < 0 && y < 0) return theta + M_PI;
	return theta;
}

OIMO::Euler MatrixToEuler2(const Eigen::Matrix3d& mtx)
{
	const double* te = mtx.data();
	float x, y, z;
	if (te[3] > 0.998f) { // singularity at north pole
		y = std::atan2(te[2], te[8]);
		z = M_PI_2;
		x = 0;
	}
	else if (te[3] < -0.998f) { // singularity at south pole
		y = std::atan2(te[2], te[8]);
		z = -M_PI_2;
		x = 0;
	}
	else {
		y = std::atan2(-te[6], te[0]);
		x = std::atan2(-te[5], te[4]);
		z = std::asin(te[3]);
	}
	return OIMO::Euler(x, y, z);
}
//这里重要，线程间隔100ms读取里程计信息，回调该方法。
//获取里程计之后读取激光数据，以此方式实现里程计和激光数据的时间同步。
//使用该方法前需要先打开激光，连接机器人
void AxRobotOdometry::robotInfo(ArNetPacket* packet)
{
	/* Extract the data from the update packet. Its format is status and
	* mode (null-terminated strings), then 6 doubles for battery voltage, 
	* x position, y position and orientation (theta) (from odometry), current
	* translational velocity, and current rotational velocity. Translation is
	* always milimeters, rotation in degrees.
	*/
	double myX_1;
	double myY_1;
	double myTh_1;
	double myVel_1;
	double myRotVel_1;
	double myVoltage_1;
	char myStatus_1[256];
	char myMode_1[32];

	memset(myStatus_1, 0, sizeof(myStatus_1));
	memset(myMode_1, 0, sizeof(myMode_1));
	packet->bufToStr(myStatus_1, sizeof(myStatus_1));
	packet->bufToStr(myMode_1, sizeof(myMode_1));
	myVoltage_1 = ( (double) packet->bufToByte2() )/10.0;
	myX_1 = (double) packet->bufToByte4();
	myY_1 = (double) packet->bufToByte4();
	myTh_1 = (double) packet->bufToByte2();
	myVel_1 = (double) packet->bufToByte2();
	myRotVel_1 = (double) packet->bufToByte2();

	fflush(stdout);
	/////////////////////////////////////////////////////////////////////
	double theta=myTh_1*M_PI/180;
	if (mobileSim==false)
	{
		odometry=OrientedPoint(myX_1/1000, myY_1/1000, theta);//存储的是弧度
		extrapolator->AddOdometry(odometry);
		m_Goal = extrapolator->getGoal();
		m_Extrapolator_Pose = extrapolator->extrapolator_pose();
		//抛出消息，进行绘制实时的激光和里程计
		emit LaserReceived(&m_Extrapolator_Pose);
		//设置的目标点和推算的位姿以mm为单位
		double deltax = (m_Goal.getX() - m_Extrapolator_Pose.getX()) / 1000;
		double deltay = (m_Goal.getY() - m_Extrapolator_Pose.getY()) / 1000;
		double distance=sqrt(deltax*deltax+deltay*deltay);
		oldDistance=distance;
		bool flag4going=false;
		if (m_GoalSetType==GOAL_SET_TYPE_AUTOEXPLORER)
		{
			double targetTheta=atan2(deltay,deltax);//弧度角（-PI，PI）
			bool tmp = distance<3.0 && abs(targetTheta - m_Extrapolator_Pose.getThRad())<M_PI / 3;
			
			//如果机器人卡死了，强制寻找新的目标点
			//if (oldDistance-distance<0.2)
			//{
			//}
			//if (false)//卡死了
			//{
			//	m_isGoalSet=false;
			//	emit goalDone();//线程抛出消息，接收完毕
			//}
			flag4going=(m_isGoalSet==true && !tmp);
		}
		else if (m_GoalSetType==GOAL_SET_TYPE_MOUSE)
		{
			flag4going=(m_isGoalSet==true && distance>0.5);
		}
		if (flag4going)
		{
			control_signal = extrapolator->getLatestControlSignal();
			long currentSec = QDateTime::currentMSecsSinceEpoch();
			if (firstMoveCtrl == 0)
			{
				this->flag = ROBOT_GOTOHEADING;
				lastSecMoveControl = currentSec;
				firstMoveCtrl++;
			}
			if (currentSec - lastSecMoveControl >= 1000)
			{
				this->flag = ROBOT_GOTOHEADING;
				lastSecMoveControl = currentSec;
			}
		}
		else
		{
			m_isGoalSet=false;
			//发送消息，已经到达目标。
			emit goalDone();//线程抛出消息，接收完毕
		}	
	}
	else
	{
		odometry=OrientedPoint(myX_1/1000, myY_1/1000, theta);//存储的是弧度
		extrapolator->AddOdometry(odometry);
	}
}



void AxRobotOdometry::laserCurrent(ArNetPacket* packet)
{
	//using namespace std;
	char sensorName[50];
	int laserX=0,laserY=0;
	std::ofstream write;
	memset(sensorName, 0, sizeof(sensorName));
	memset(laserReadingX, 0, sizeof(laserReadingX));
	memset(laserReadingY, 0, sizeof(laserReadingY));

	laserReadingNo = packet->bufToByte2();   // Number of laser data with X & Y
	packet->bufToStr(sensorName, sizeof(sensorName)); // Sensor's name

	for(int i=0;i<laserReadingNo;i++)
	{ 
		laserX = packet->bufToByte4();  // X coordinate for laser reading
		laserY = packet->bufToByte4();  // Y coordinate for laser reading

		laserReadingX[i] = laserX;
		laserReadingY[i] = laserY;
	}
	fflush(stdout);
	if (mobileSim==true)
	{
		double res=M_PI*3/2/(laserReadingNo-1);
		string sensorName="FLASER";
		m_Laser=new RangeSensor("lms1xx",laserReadingNo, res, OrientedPoint(0,0,0), 0, 20);
		m_Laser->updateBeamsLookup();
		if (m_sensorMap==NULL)
		{
			m_sensorMap = &(extrapolator->m_sensorMap);
			extrapolator->setLaserReadingNo(laserReadingNo);
		}
		m_sensorMap->insert(make_pair(sensorName, m_Laser));//目前这种构造方法有问题！！
		const RangeSensor * rs=m_Laser;
		RangeReading reading(rs, 1);
		reading.resize(rs->beams().size());
		int beamsize=rs->beams().size();
		if (laserReadingNo==beamsize)
		{
			for(int i=0;i<laserReadingNo;i++)
			{ 
				double x=laserReadingX[i];
				double y=laserReadingY[i];
				double rx =odometry.x;
				double ry =odometry.y;
				double dx = x/1000 - rx;		
				double dy = y/1000 - ry;
				double dist2 = dx*dx + dy*dy;
				reading[i]=sqrt(dist2);
			}
		}
		else if (laserReadingNo<beamsize)
		{
			for(int i=0;i<beamsize;i++)
			{ 
				if (i<laserReadingNo)
				{
					double x=laserReadingX[i];
					double y=laserReadingY[i];
					double rx =odometry.x;
					double ry =odometry.y;
					double dx = x/1000 - rx;		
					double dy = y/1000 - ry;
					double dist2 = dx*dx + dy*dy;
					reading[i]=sqrt(dist2);
				}
				else
				{
					reading[i]=-1;
				}
			}
		}
		else if (laserReadingNo > beamsize)
		{
			for(int i=0;i< beamsize;i++)
			{ 
				double x=laserReadingX[i];
				double y=laserReadingY[i];
				double rx =odometry.x;
				double ry =odometry.y;
				double dx = x/1000 - rx;		
				double dy = y/1000 - ry;
				double dist2 = dx*dx + dy*dy;
				reading[i]=sqrt(dist2);
			}
		}
		extrapolator->VFF(reading);
		m_Goal = extrapolator->getGoal();
		m_Extrapolator_Pose = extrapolator->extrapolator_pose();

		//抛出消息，进行绘制实时的激光和里程计
		emit LaserReceived(&m_Extrapolator_Pose);

		double deltax = (m_Goal.getX() - m_Extrapolator_Pose.getX()) / 1000;
		double deltay = (m_Goal.getY() - m_Extrapolator_Pose.getY()) / 1000;
		double distance=sqrt(deltax*deltax+deltay*deltay);
		bool flag4going=false;
		if (m_GoalSetType==GOAL_SET_TYPE_AUTOEXPLORER)
		{
			double targetTheta=atan2(deltay,deltax);//弧度角（-PI，PI）
			bool tmp = distance<3.0 && abs(targetTheta - m_Extrapolator_Pose.getThRad())<M_PI / 3;
			flag4going=(m_isGoalSet==true && !tmp);
		}
		else if (m_GoalSetType==GOAL_SET_TYPE_MOUSE)
		{
			flag4going=(m_isGoalSet==true && distance>0.5);
		}
		if (flag4going)
		{
			control_signal = extrapolator->getLatestControlSignal();
			long currentSec = QDateTime::currentMSecsSinceEpoch();
			if (firstMoveCtrl == 0)
			{
				this->flag = ROBOT_GOTOHEADING;
				lastSecMoveControl = currentSec;
				firstMoveCtrl++;
			}
			if (currentSec - lastSecMoveControl >= 1000)
			{
				this->flag = ROBOT_GOTOHEADING;
				lastSecMoveControl = currentSec;
			}
		}
		else
		{
			m_isGoalSet=false;
			//发送消息，已经到达目标。
			emit goalDone();//线程抛出消息，接收完毕
		}
		
		//QDateTime  now=QDateTime ::currentMSecsSinceEpoch();
		long currentSec=QDateTime ::currentMSecsSinceEpoch();//long(())*1000+now.msec();
		if (firstLaser==0)
		{
			extrapolator->AddRangeReading(reading);
			lastSec=currentSec;
			firstLaser++;
		}
		if (currentSec-lastSec>=1000)
		{
			extrapolator->AddRangeReading(reading);
			lastSec=currentSec;
		}
		
	}
}

